﻿/*
    Copyright 2010, 2011 Eric Wong
	contact@optimalcycling.com
	http://www.optimalcycling.com
	Optimal Cycling - Advanced power pacing program for cyclists
	
	This file is part of Optimal Cycling.

    Optimal Cycling is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Optimal Cycling is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with Optimal Cycling.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef __OPENCL_VERSION__
	#include "ObjectiveFunc.h"
#endif

inline real gmVal(__global const real* time,
				  __global const real* currPowerVals,
				  __global const PowerMetricsInfo* powerMetricsInfo,
				  const sizeT numSimPoints)
{
	// Vars for GM calculations
	real samplingRate = powerMetricsInfo->gmSamplingInterval;
	sizeT totalPoints = ceil((time[numSimPoints - 1] - time[0]) / samplingRate) + 1;
	sizeT numPtsForAveraging = clamp(ceil(powerMetricsInfo->gmRollingAveTime / samplingRate), 1, totalPoints);
	
	if (powerMetricsInfo->gmRollingAveIsExp)
	{
		real sumAveRoot, currTimeTemp;
		sizeT pointsUsed, i;

		real expAlpha = TWO / ((powerMetricsInfo->gmRollingAveTime / samplingRate) + ONE);
		real expAveVal = 0;
		real sumAveRootCorrection = 0;
		real expAveValCorrection = 0;

		sumAveRoot = 0;
		pointsUsed = 0;
		i = 0;

		currTimeTemp = i * samplingRate + time[0];
		expAveVal = InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints);
		sumAveRoot = sumAveRoot + pow(expAveVal, powerMetricsInfo->gmExp);
		pointsUsed++;

		for (i = 1; i < totalPoints; i++)
		{
			currTimeTemp = (i - 1) * samplingRate + time[0];
			
			#ifdef KAHAN_SUMMATION
				KahanSum(&expAveVal, &expAveValCorrection, NEG_ONE * expAlpha * expAveVal);
				KahanSum(&expAveVal, &expAveValCorrection, expAlpha * InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints));
				KahanSum(&sumAveRoot, &sumAveRootCorrection, pow(expAveVal, powerMetricsInfo->gmExp));
			#else
				expAveVal = expAveVal + expAlpha * (InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints) - expAveVal);
				sumAveRoot = sumAveRoot + pow(expAveVal, powerMetricsInfo->gmExp);
			#endif

			pointsUsed++;
		}

		return pow(real_divide(sumAveRoot, pointsUsed), powerMetricsInfo->gmExpInverse);
	}
	else
	{
		real sum, sumAveRoot, currTimeTemp;
		sizeT pointsUsed, i;

		real sumCorrection = 0;
		real sumAveRootCorrection = 0;

		sumAveRoot = 0;
		pointsUsed = 0;
		sum = 0;

		// Find the sum of numPtsForAveraging first					
		for (i = 0; i < numPtsForAveraging; i++)
		{
			currTimeTemp = i * samplingRate + time[0];

			#ifdef KAHAN_SUMMATION
				KahanSum(&sum, &sumCorrection, InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints));				
			#else
				sum = sum + InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints);
			#endif
		}
		#ifdef KAHAN_SUMMATION
			KahanSum(&sumAveRoot, &sumAveRootCorrection, pow(sum / numPtsForAveraging, powerMetricsInfo->gmExp));
		#else
			sumAveRoot = sumAveRoot + pow(sum / numPtsForAveraging, powerMetricsInfo->gmExp);
		#endif
		pointsUsed++;

		// Now calculate the sumAveRoot for each additional power data point
		for (; i < totalPoints; i++)
		{
			currTimeTemp = i * samplingRate + time[0];

			#ifdef KAHAN_SUMMATION
				KahanSum(&sum, &sumCorrection, InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints));
				KahanSum(&sum, &sumCorrection, NEG_ONE * InterpolateBinary((i - numPtsForAveraging) * samplingRate + time[0], time, currPowerVals, numSimPoints));
				KahanSum(&sumAveRoot, &sumAveRootCorrection, pow(sum / numPtsForAveraging, powerMetricsInfo->gmExp));
			#else
				sum = sum + InterpolateBinary(currTimeTemp, time, currPowerVals, numSimPoints)
					- InterpolateBinary((i - numPtsForAveraging) * samplingRate + time[0], time, currPowerVals, numSimPoints);
				sumAveRoot = sumAveRoot + pow(sum / numPtsForAveraging, powerMetricsInfo->gmExp);
			#endif

			pointsUsed++;
		}

		return pow(real_divide(sumAveRoot, pointsUsed), powerMetricsInfo->gmExpInverse);
	}
}

inline real ObjectiveFunc(
		__global real* time,
		__global const real* currPowerVals,
		__global const real2* powerLimits,

		__global const real4* iConstants,	// real Tol {s0}, Hmin {s1}, Hmax {s2}, Hstart {s3};
		const real currMaxTime,

		__global const SolverVars* solverVars,
		__global const PowerMetricsInfo* powerMetricsInfo,
		__global const real2* powerMetricsCriticals,

		__global const real4* coursePosPoints, 
		__global const real* coursePosLinear,
		const real coursePosLinearMax,
		__global const real4* coursePosDirection,
		__global const real4* coursePosInfo, 
		__global const real4* riderProp,
		__global const real4* wind, 
		__global const real4* climate,
		const sizeT numSimPoints,

		__global const real* dragCoeffWheelYawAngles,
		__global const real2* dragCoeffWheels, 
		const sizeT dragCoeffWheelsNumPts,

		__global const ModelInfo* modelInfo,
		
		__global real8* bestPowerResults,
		__global real8* bestKinematics	)
{	
	// Initialize the integrator
	real4 iData = {0, 0, modelInfo->initialSpeed, 0};	// iTime {s0}, iPosition {s1}, iSpeed {s2}, iAccel {s3}
	real iPower, pmSum, pmSum2, pmVal, criticalPMVal, prevTime, prevPos, lastPosSuccess;
	real pmAveSum, pmAveSum2, pmGMSum;
	real currTime, prevTimeTemp, targetPos, currSpeed, prevSpeed, overSpeedSum;
	real overPowerSum;
	real currPowerRate;
	real raceTimeStart;
	int result;
	real4 iParams1;	// real t0 {s0}, y0 {s1}, dy0 {s2}, d2y0 {s3}
	real2 iParams2;	// H {s0}, lastDecreasedH {s1}
	sizeT counter = 0;
	sizeT i, j, numPMTypes;
	real timeDiff, posOver;
	real prevAccel;

	real pmSumCorrection = 0;
	real pmSum2Correction = 0;
	real pmAveSumCorrection = 0;
	real pmAveSum2Correction = 0;
	real4 iCorrectionData = {0, 0, 0, 0};	

	result = ModelAcceleration(&iData, &iPower, currPowerVals, KERNEL_VARS_TO_MODEL);
	if (result == 0)
		return (currMaxTime + iData.s0 + 2 * coursePosLinearMax);

	iParams1.s0 = 0;
	iParams1.s1 = 0;
	iParams1.s2 = iData.s2;
	iParams1.s3 = iData.s3;

	iParams2.s0 = (*iConstants).s3;
	iParams2.s1 = -1;

	currTime = iData.s0;
	raceTimeStart = iData.s0;
	prevTime = iData.s0;
	prevTimeTemp = prevTime;
	prevPos = iData.s1;
	lastPosSuccess = prevPos;
	currSpeed = iData.s2;
	prevSpeed = iData.s2;
	overSpeedSum = 0;
	pmSum = 0;
	pmSum2 = 0;
	pmAveSum = 0;
	pmAveSum2 = 0;
	pmGMSum = 0;
	overPowerSum = 0;
	time[counter] = currTime;

	/// <param name="bestPowerResults">Length of 1, {s0} Energy sum [J], {s1} Average power [watts], {s2} CCAP [watts], {s3} CCAP critical [watts]</param>  
	/// <param name="bestKinematics">Length of numSimPoints, {s0} power values [watts], {s1} Time points [sec], {s2} speed [m/s], {s3} acceleration [m/s^2]</param>  
	if (bestPowerResults)
	{
		bestPowerResults[0].s0 = LARGE_REAL;
		bestPowerResults[0].s1 = LARGE_REAL;
		bestPowerResults[0].s2 = LARGE_REAL;
		bestPowerResults[0].s3 = LARGE_REAL;

		numPMTypes = PMTypes_NUM_COUNT;
	}
	else
	{
		numPMTypes = 1;
	}
	
	if (bestKinematics)
	{
		for (i = 0; i < numSimPoints; i++)
		{
			bestKinematics[i].s0 = currPowerVals[i];	// Raw power
			bestKinematics[i].s1 = LARGE_REAL;	// Time [sec]
			bestKinematics[i].s2 = LARGE_REAL;	// speed [m/s]
			bestKinematics[i].s3 = LARGE_REAL;	// acceleration [m/s^2]
		}

		prevAccel = iData.s3;

		bestKinematics[counter].s1 = iData.s0;
		bestKinematics[counter].s2 = iData.s2;
		bestKinematics[counter].s3 = iData.s3;		
	}

	counter++;
	targetPos = coursePosLinear[counter];	

	while (result != -1 && iData.s0 < currMaxTime && iData.s1 < coursePosLinearMax)
	{
		result = RKF45Step(&iData, &iCorrectionData, &iParams1, &iParams2, iConstants, &iPower, currPowerVals, KERNEL_VARS_TO_MODEL);

		if (result == 0)
		{			
			while (counter < numSimPoints && iData.s1 >= targetPos)
			{							
				posOver = iData.s1 - prevPos;

				if (posOver < SMALL_REAL)
				{
					currTime = iData.s0;
					currSpeed = iData.s2;

					if (bestKinematics)
					{
						bestKinematics[counter].s1 = iData.s0;
						bestKinematics[counter].s2 = iData.s2;
						bestKinematics[counter].s3 = iData.s3;
					}
				}
				else
				{
					real controlVal = real_divide(targetPos - prevPos, posOver);						
					currTime = mix(prevTime, iData.s0, controlVal);
					currSpeed = mix(prevSpeed, iData.s2, controlVal);

					if (bestKinematics)
					{
						bestKinematics[counter].s1 = mix(prevTime, iData.s0, controlVal);
						bestKinematics[counter].s2 = mix(prevSpeed, iData.s2, controlVal);
						bestKinematics[counter].s3 = mix(prevAccel, iData.s3, controlVal);	
					}
				}

				overSpeedSum += overMax(currSpeed, coursePosInfo[counter].s3);

				timeDiff = currTime - prevTimeTemp;

				if (counter == solverVars->raceTimeMeasureStartIndex)
					raceTimeStart = currTime;

#ifdef CCAP_RATE_TRANSFORMS
				currPowerRate = real_divide(currPowerVals[counter] - currPowerVals[counter - 1], timeDiff);				

				if (currPowerRate >= 0)
				{
			#ifdef CCAP_MAXIMAL_POWER_RATES
					if (currPowerRate > powerMetricsInfo->maxPosPowerRate)
					{
						overPowerSum += currPowerRate;
					}
			#endif
					currPowerRate *= powerMetricsInfo->posFactor;
				}
				else
				{
			#ifdef CCAP_MAXIMAL_POWER_RATES
					if (currPowerRate < powerMetricsInfo->minNegPowerRate)
					{
						overPowerSum += (-1 * currPowerRate);
					}
			#endif
					currPowerRate *= powerMetricsInfo->negFactor;
				}
#else
				currPowerRate = fabs(real_divide(currPowerVals[counter] - currPowerVals[counter - 1], timeDiff));				
#endif	// CCAP_RATE_TRANSFORMS


				for(j = 0; j < numPMTypes; j++)
				{
					PMTypes currPMType = (numPMTypes == 1 ? powerMetricsInfo->pmType : (PMTypes)j);
					switch(currPMType)
					{
						case PMTypes_CCAP:
							#ifdef KAHAN_SUMMATION
									KahanSum(&pmSum, &pmSumCorrection, timeDiff * (ONE_HALF * (currPowerVals[counter] * pow(currPowerVals[counter] / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp)
										+ currPowerVals[counter - 1] * pow(currPowerVals[counter - 1] / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp))));
									KahanSum(&pmSum2, &pmSum2Correction, timeDiff * pow(currPowerRate, powerMetricsInfo->powerRateExp));
							#else
									pmSum += timeDiff * (ONE_HALF * (currPowerVals[counter] * pow(currPowerVals[counter] / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp)
										+ currPowerVals[counter - 1] * pow(currPowerVals[counter - 1] / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp)));
									pmSum2 += timeDiff * pow(currPowerRate, powerMetricsInfo->powerRateExp);
							#endif		// KAHAN_SUMMATION
							break;

						case PMTypes_Average:
							#ifdef KAHAN_SUMMATION
									KahanSum(&pmAveSum, &pmAveSumCorrection, (currPowerVals[counter] + currPowerVals[counter - 1]) * timeDiff);
									KahanSum(&pmAveSum2, &pmAveSum2Correction, timeDiff * currPowerRate);		
							#else
									pmAveSum += (currPowerVals[counter] + currPowerVals[counter - 1]) * timeDiff;
									pmAveSum2 += timeDiff * currPowerRate;
							#endif		// KAHAN_SUMMATION
							
							break;

						case PMTypes_GeneralMean:								
							time[counter] = currTime;

							// This section below is not actually general mean (but somewhat close). 
							// It's values are only used when the power values are not viable.
							// Once an organism becomes viable, the real general mean values are calculated and used.
							// This is done because it is simpler to calculate this form, which is the exponential of a linear line.
							if (fabs(currPowerRate) > SMALL_REAL)
								pmGMSum += real_divide( powr(currPowerVals[counter - 1] + currPowerRate * timeDiff, powerMetricsInfo->gmExpPlusOne) 
								- powr(currPowerVals[counter - 1], powerMetricsInfo->gmExpPlusOne),  powerMetricsInfo->gmExpPlusOne * currPowerRate);				
							else
								pmGMSum += powr(currPowerVals[counter - 1], powerMetricsInfo->gmExp) * timeDiff;

							break;

						default:
							break;
					}
				}

				prevTimeTemp = currTime;				
				
				lastPosSuccess = targetPos;
				counter++;

				if (counter < numSimPoints)
					targetPos = coursePosLinear[counter];
			}

			prevTime = iData.s0;
			prevPos = iData.s1;
			prevSpeed = iData.s2;
		}
	}  

	if (result == 0 && currTime <= currMaxTime && lastPosSuccess >= coursePosLinearMax 
		&& overSpeedSum <= SMALL_REAL)
	{	
		if (bestPowerResults)
		{
			criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals); 

			bestPowerResults[0].s0 = pmAveSum;
			bestPowerResults[0].s1 = real_divide(pmAveSum, currTime * 2.0f);

			// CCAP
			bestPowerResults[0].s2 = real_divide(pmSum, currTime) + powerMetricsInfo->powerRateCoeff * pow(real_divide(pmSum2, currTime), powerMetricsInfo->powerRateExpInverse);
			bestPowerResults[0].s2 = bestPowerResults[0].s2 / pow(criticalPMVal / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp);
			bestPowerResults[0].s3 = criticalPMVal;
			bestPowerResults[0].s4 = gmVal(time, currPowerVals, powerMetricsInfo, numSimPoints);
			bestPowerResults[0].s5 = currTime;
		}

		switch(powerMetricsInfo->pmType)
		{
			case PMTypes_CCAP:
				pmVal = real_divide(pmSum, currTime) + powerMetricsInfo->powerRateCoeff * pow(real_divide(pmSum2, currTime), powerMetricsInfo->powerRateExpInverse);
				criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals);
				criticalPMVal = criticalPMVal * pow(criticalPMVal / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp);  
				break;
			case PMTypes_Average:	
				// Small additive value to help the solver find the best power for average power.
				// This should not have a significant material effect.
				// Without it, average power is very hard to minimize.	
				pmVal = real_divide(pmAveSum, currTime * 2.0f) + 0.1f * real_divide(pmAveSum2, currTime);
				criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals); 
				break;
			case PMTypes_GeneralMean:
				pmVal = gmVal(time, currPowerVals, powerMetricsInfo, numSimPoints);
				criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals); 
				break;
			default:
				pmVal = 0;
		}

		if (bestPowerResults)
		{
			// There is a rounding issue here when the power metric values are calculated again from array data that has been copied
			// because of the way real numbers are represented in memory. Hence the 0.01f added to criticalPMVal when comparing the final data,
			// which has been recalculated.
			if (pmVal > (criticalPMVal + 0.01f) || overPowerSum > SMALL_REAL)						
				return currMaxTime + currTime
					+ fabs(coursePosLinearMax - lastPosSuccess) + fabs(pmVal - criticalPMVal) + overPowerSum;
			else
				return (currTime - raceTimeStart);
		}
		else
		{
			if (pmVal > criticalPMVal || overPowerSum > SMALL_REAL)						
				return currMaxTime + currTime
					+ fabs(coursePosLinearMax - lastPosSuccess) + fabs(pmVal - criticalPMVal) + overPowerSum;
			else
				return (currTime - raceTimeStart);
		}
	}
	else
	{
		if (bestPowerResults)
		{
			criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals); 

			bestPowerResults[0].s0 = pmAveSum;
			if (currTime >= SMALL_REAL)
			{
				bestPowerResults[0].s1 = real_divide(pmAveSum, currTime);

				// CCAP
				bestPowerResults[0].s2 = real_divide(pmSum, currTime) + powerMetricsInfo->powerRateCoeff * pow(real_divide(pmSum2, currTime), powerMetricsInfo->powerRateExpInverse);
				bestPowerResults[0].s2 = bestPowerResults[0].s2 / pow(criticalPMVal / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp);
			}
			else
			{
				bestPowerResults[0].s1 = 0;
				bestPowerResults[0].s2 = 0;
			}
			bestPowerResults[0].s3 = criticalPMVal;
			bestPowerResults[0].s4 = pow(real_divide(pmGMSum, currTime), powerMetricsInfo->gmExpInverse);
			bestPowerResults[0].s5 = solverVars->maxTime;
		}

		switch(powerMetricsInfo->pmType)
		{
			case PMTypes_CCAP:
				if (currTime >= SMALL_REAL)
					pmVal = real_divide(pmSum, currTime) + powerMetricsInfo->powerRateCoeff * pow(real_divide(pmSum2, currTime), powerMetricsInfo->powerRateExpInverse);
				else
					pmVal = 0;

				criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals);
				criticalPMVal = criticalPMVal * pow(criticalPMVal / powerMetricsInfo->ftp, powerMetricsInfo->powerCurveExp);  
				break;
			case PMTypes_Average:
				if (currTime >= SMALL_REAL)
					pmVal = real_divide(pmAveSum, currTime);
				else
					pmVal = 0;

				criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals);
				break;
			case PMTypes_GeneralMean:
				if (currTime >= SMALL_REAL)
					pmVal = pow(real_divide(pmGMSum, currTime), powerMetricsInfo->gmExpInverse);
				else
					pmVal = 0;

				criticalPMVal = InterpolateBinaryReal2(currTime, powerMetricsCriticals, powerMetricsInfo->numCriticals);
				break;
			default:
				pmVal = 0;
		}

		if (result == 0 && (currTime <= currMaxTime || lastPosSuccess >= coursePosLinearMax))
		{
			return TWO * currMaxTime + currTime
				+ fabs(coursePosLinearMax - lastPosSuccess) + fabs(pmVal - criticalPMVal) + overSpeedSum + overPowerSum;
		}
		else
		{
			return FOUR * currMaxTime + currTime
				+ fabs(coursePosLinearMax - lastPosSuccess) + fabs(pmVal - criticalPMVal) + overSpeedSum + overPowerSum;
		}
	}		
}


